import numpy as np
from stereo.utils.Filter.one_euro_filter import OneEuroFilter as CppOneEuroFilter

class OneEuroFilter:
    def __init__(self, te, mincutoff=1.0, beta=0.007, dcutoff=0.5):
        self.filter = CppOneEuroFilter(te, mincutoff, beta, dcutoff)

    def __call__(self, x, te):
        return self.filter.filter(x, te)

if __name__ == '__main__':
    filter = OneEuroFilter(te=0.01)
    points2d = np.random.randint(0, 100, (32, 2))
    import time
    t1 = time.time()
    for i in range(1000):
        points2d = filter(points2d, 0.01)
    t2 = time.time()
    print(t2-t1)
    print(points2d.shape)
    filter = OneEuroFilter(te=0.01)
    points3d = np.random.randint(0, 100, (32, 3))
    points3d = filter(points3d, 0.01)
    print(points3d.shape)